% MonteCarloMain
clc
clear
close all
load('MC23.mat')
curN = 24;
global m S d2r R0 g0 rot Vs ts Vup Vdown alphamax alphamin alphaLD dAziUp dAziDown...
       dtInt dtGui N ef absBankMax Vf
d2r = pi/180;
S = 0.48;
R0 = 6378135;
g0 = 9.81;
Vs = sqrt(g0*R0);
ts = sqrt(R0/g0);
rot = 7.292115E-5;
alphamax = 20*d2r;
alphaLD = 12*d2r;
alphamin = 5*d2r;
absBankMax = 60*pi/180;
Vup = 5900/Vs;
Vdown = 5000/Vs;
dAziUp = 10*d2r;
dAziDown = -10*d2r;
dtInt = 0.1/ts; % 积分周期
dtGui = 1/ts; % 制导周期
N = 1000;

Vf = 2000/Vs;
tf = 1460/ts;
lonf = 50*d2r;
latf = 50*d2r;
hf = 30e3;
m = 907;
lon0 = 6.5*d2r;
lat0 = 7.5*d2r;
h0 = 72e3;
V0 = 6100/Vs;
fpa0 = -1*d2r;
azi0 = 31*d2r;

for number = curN:200
    % 拉偏参数
    lon0 = normrnd(6.5*d2r,0.5*d2r);
    lat0 = normrnd(7.5*d2r,0.5*d2r);
%     h0 = normrnd(72e3,1e3);
%     V0 = normrnd(6100/Vs, 30/Vs);
%     fpa0 = normrnd(-1*d2r, 0.01*d2r);
%     azi0 = normrnd(31*d2r, 0.1*d2r);

    rad0 = (R0 + h0)/R0;
    radf = (R0 + hf)/R0;
    e0 = 1/rad0 - V0^2/2;
    ef = 1/radf - Vf^2/2;
    Stogo0 = acos(sin(lat0)*sin(latf) + cos(lat0)*cos(latf)*cos(lonf-lon0));
    p = 1;
    state(p,:) = [rad0, lon0, lat0, V0, fpa0, azi0];
    statef = [radf, lonf, latf, Vf, nan, nan];
    time(p,1) = 0;
    ctrl(p,:)= [20*d2r, 0, 1]; % 攻角，倾侧角幅值，倾侧角符号
    auxState(p,:) = [e0 Stogo0 0 0 0 0 0 0 0]; % e stogo bankbase bankf dAzi iter F G kh
    k = dtGui/dtInt;
    fpaflag = false(1);
    while 1
        statet = RK4(@ReentryDynamic, state(p,:), ctrl(p,:), dtInt);
        r = statet(1);
        V = statet(4);
        fpa = statet(5);
        if V > Vup
            alphar = alphamax;
        elseif V > Vdown
            alphar = (alphamax - alphaLD)*(V - Vdown)/(Vup - Vdown) + alphaLD;
        else
            alphar = alphaLD;
        end
        if k <= 0
            if fpaflag
                [ctrl_temp, aux] = MCGuidanceLoop(time(p,1), tf, state(p,:),...
                    statef, ctrl(p,:), auxState(p,2:9), alphar);
                alpha = limitRate(ctrl_temp(1), ctrl(p,1), dtGui*ts, 2*pi/180);
                bank = limitRate(ctrl_temp(2), ctrl(p,2), dtGui*ts, 10*pi/180);
                ctrl_temp = [alpha bank ctrl_temp(3)];
            else
                ctrl_temp = [alphar 0 1];
            end
            if tf - time(p,1) < 0.5
                k = 5;
            else
                k = dtGui/dtInt;
            end
        else
            ctrl_temp = ctrl(p,:);
        end
        ctrl(p+1,:) = ctrl_temp;
        state(p+1,:) = statet;
        time(p+1,1) = time(p,1) + dtInt;
        if fpa > 0
            fpaflag = true(1);
        end
        e = 1/r - V^2/2;
        auxState(p+1,:) = [e Stogo0 30*d2r 30*d2r 0 0 0 0 0];
        if exist('aux', 'var')
            auxState(p+1,2:9) = aux;
        end
        s = auxState(p+1,2);
        p = p+1;
        k = k-1;
        if e >= ef % V <= Vf % || s <= 0
            V = state(:, 4)*Vs;
            deltaV(1,number) = V(end) - Vf*Vs;
            alt = (state(:, 1) - 1)*R0/1000;
            deltaH(1,number) = alt(end) - hf/1000;
            lon = state(:, 2)*180/pi;
            lat = state(:, 3)*180/pi;
            deltaS(1,number) = acos(sin(lat(end)*pi/180)*sin(latf) + cos(lat(end)*pi/180)*cos(latf)*cos(lonf-lon(end)*pi/180))*R0/1000;
            t = ts*time;
            deltat(1,number) = t(end) - tf*ts;
            tMC(1:size(lon), number) = t;
            lonMC(1:size(lon), number) = lon;
            latMC(1:size(lat), number) = lat;
            altMC(1:size(alt), number) = alt;
            break;
        end
    end
end
save('MC.mat', 'deltaV');
save('MC.mat', 'deltaH','-append');
save('MC.mat', 'deltaS','-append');
save('MC.mat', 'deltat','-append');
save('MC.mat', 'tMC','-append');
save('MC.mat', 'lonMC','-append');
save('MC.mat', 'latMC','-append');
save('MC.mat', 'altMC','-append');

%% plot
t = ts*time;
alt = (state(:, 1) - 1)*R0/1000;
lon = state(:, 2)*180/pi;
lat = state(:, 3)*180/pi;
V = state(:, 4)*Vs;
fpa = state(:, 5)*180/pi;
azi = state(:, 6)*180/pi;
alpha = ctrl(:, 1)*180/pi;
absBank = ctrl(:, 2)*180/pi;
signOfBank = ctrl(:, 3);
bank = absBank.*signOfBank;
% e stogo bankbase bankf dAzi iter F G
bankbase = auxState(:, 3)*180/pi;
bankf = auxState(:, 4)*180/pi;
iter = auxState(:, 6);
Serror = auxState(:, 7);
Terror = auxState(:, 8);
kh = auxState(:, 9);

f = figure('Name', '三维轨迹');
plot3(lon, lat, alt, 'linewidth', 2);
xlabel('Longitude');ylabel('Latitude');zlabel('Altitude/km');
grid on
saveas(f, f.Name, 'svg')

f = figure('Name', '地面轨迹');
plot(lon, lat, 'linewidth', 1.5);
xlabel('经度/(deg)'); ylabel('纬度/(deg)');
saveas(f, f.Name, 'svg')

f = figure('Name', '高度-时间历程');
plot(t, alt, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('高度/km');
saveas(f, f.Name, 'svg')

f = figure('Name', '速度-时间历程');
plot(t, V, 'linewidth', 1.5);
xlabel('时间/s)'); ylabel('速度/(m·s^{-1}）');
saveas(f, f.Name, 'svg')

f = figure('Name', '航迹角-时间历程');
plot(t, fpa, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('航迹角/(deg)');
saveas(f, f.Name, 'svg')

f = figure('Name', '航向角-时间历程');
plot(t, azi, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('航向角/(deg)');
saveas(f, f.Name, 'svg')

f = figure('Name', '攻角-时间历程');
plot(t, alpha, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('攻角/(deg)');
saveas(f, f.Name, 'svg')

f = figure('Name', '倾侧角-时间历程');
plot(t, bank, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('倾侧角/(deg)');
saveas(f, f.Name, 'svg')

figure
plot(t, signOfBank, 'o');
xlabel('时间/s'); ylabel('Sign Of Bank Angle (deg)');

figure
plot(t, bankbase, t, bankf, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('bankprofile (deg)');
legend('bankc', 'bankf');

figure
plot(t, iter, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('iterations (deg)');

figure
plot(t, Serror, t, Terror, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('error');
legend('Serror', 'Terror');